#define IN1 12
#define IN2 4
#define ENA 11
#define IN3 7
#define IN4 8
#define ENB 9

const int pwmPin1 = 3;  // 方向通道
const int pwmPin2 = 2;  // 油门通道

volatile unsigned long risingTime1, risingTime2;
volatile unsigned long pulseWidth1 = 1500, pulseWidth2 = 1500; // 初始值设为中位
unsigned long lastUpdateTime = 0;

// 电机控制函数
void setLeftMotor(int speed) {
  speed = constrain(speed, -255, 255);
  if(speed > 0) {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
  } else {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  }
  analogWrite(ENA, abs(speed));
}

void setRightMotor(int speed) {
  speed = constrain(speed, -255, 255);
  if(speed > 0) {
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
  } else {
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
  }
  analogWrite(ENB, abs(speed));
}

void stopMotors() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}

// 中断处理函数
void risingEdge1() {
  risingTime1 = micros();
  attachInterrupt(digitalPinToInterrupt(pwmPin1), fallingEdge1, FALLING);
}

void fallingEdge1() {
  unsigned long width = micros() - risingTime1;
  if(width >= 800 && width <= 2200) { // 有效PWM范围检查
    pulseWidth1 = width;
    lastUpdateTime = millis();
  }
  attachInterrupt(digitalPinToInterrupt(pwmPin1), risingEdge1, RISING);
}

void risingEdge2() {
  risingTime2 = micros();
  attachInterrupt(digitalPinToInterrupt(pwmPin2), fallingEdge2, FALLING);
}

void fallingEdge2() {
  unsigned long width = micros() - risingTime2;
  if(width >= 800 && width <= 2200) {
    pulseWidth2 = width;
    lastUpdateTime = millis();
  }
  attachInterrupt(digitalPinToInterrupt(pwmPin2), risingEdge2, RISING);
}

void setup() {
  Serial.begin(115200);
  
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENB, OUTPUT);
  
  pinMode(pwmPin1, INPUT);
  pinMode(pwmPin2, INPUT);
  
  attachInterrupt(digitalPinToInterrupt(pwmPin1), risingEdge1, RISING);
  attachInterrupt(digitalPinToInterrupt(pwmPin2), risingEdge2, RISING);
  
  stopMotors();
}

void loop() {
  // 信号丢失检测
  if(millis() - lastUpdateTime > 500) {
    stopMotors();
    return;
  }

  // 转换为-255到255的范围
  int throttle = map(pulseWidth2, 1000, 2000, -255, 255);
  int steering = map(pulseWidth1, 1000, 2000, 255, -255);

  // 死区处理
  if(abs(throttle) < 30) throttle = 0;
  if(abs(steering) < 30) steering = 0;

  // 差速转向计算
  int leftSpeed = throttle + steering;
  int rightSpeed = throttle - steering;

  // 限幅处理
  leftSpeed = constrain(leftSpeed, -255, 255);
  rightSpeed = constrain(rightSpeed, -255, 255);

  // 电机控制
  setLeftMotor(leftSpeed);
  setRightMotor(rightSpeed);

  // 调试输出
  Serial.print("Throttle: ");
  Serial.print(throttle);
  Serial.print(" Steering: ");
  Serial.println(steering);
  
  delay(20);
}